
#ifndef MY_LIB_MY_FUNCS_H_
#define MY_LIB_MY_FUNCS_H_

extern double kpUpper, kdUpper;
extern double kp;
extern double kd;
extern double ki;
extern double accu_err;
extern double pre_err;

extern double Q_gain;
extern double Q_upper;
extern double Q_lower;
extern double distance_hat0;
extern double P0;
extern double R;
extern double Q;
extern double P;
extern double distance_hat;

extern uint32_t defaultL_v;
extern uint32_t defaultR_v;
extern uint32_t deltaPIDnega;
extern uint32_t deltaPIDposi;


extern uint32_t uturnlv;
extern uint32_t uturnrv;
extern uint32_t rturnlv;
extern uint32_t rturnrv;

extern double uTurn_thresIn;
extern double rTurn_thresIn;
extern double uTurn_thresOut;
extern double rTurn_thresOut;

extern uint32_t thinLower;
extern uint32_t thinUpper;
extern uint32_t thickLower;
extern uint32_t thickUpper;

extern volatile bool TR;
extern volatile bool UT;

void uint2str(char* str, uint32_t num);
void double2str(char* str, double num);
void UARTSend(const uint8_t *pui8Buffer, const uint32_t UART_BASE);
double PID_adjustment(double error);
void robot_uturn(double distance_cmF);
void robot_TR(double distance_cmR);
void PID_followWall(double error);
double kalman_filter(double distance);

#endif /* MY_LIB_MY_FUNCS_H_ */
